#include "device.h"
#include "algorithm.h"
#include "isr.h"
#include "app_user_interface.h"
#include "app_motion_control.h"


// 定义主循环状态机
typedef enum _main_state_e
{
	GET_INFO = 0,	// 获取信息
	SHOW_INFO,		// 显示信息
	SEND_INFO,		// 发送信息
} main_state_e;
main_state_e g_main_state = GET_INFO;	// 初始化主循环状态机

// 设备驱动库
device_t device;

// 算法库
algo_imu_t algo_imu; 				// 飞行器惯性测量单元
algo_pid_t algo_pid_pitch_angle;	// 飞行器俯仰角度PID
algo_pid_t algo_pid_roll_angle;		// 飞行器横滚角度PID
algo_pid_t algo_pid_yaw_angle;		// 飞行器偏航角度PID
algo_pid_t algo_pid_pitch_gyro; 	// 飞行器俯仰角速度PID
algo_pid_t algo_pid_roll_gyro; 		// 飞行器横滚角速度PID
algo_pid_t algo_pid_yaw_gyro; 		// 飞行器偏航角速度PID

// 系统初始化
void system_init(void)
{
	// 系统中断优先级配置
	NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2); // 设置中断优先级分组，2位抢占优先级，2位响应优先级
	
	// 设备驱动初始化
	device.para.timer_frequency = 200; 			// 定时器中断周期=5ms
    device.para.motor_pwm_period = 1000;		// PWM精度
    device.para.motor_pwm_frequency = 10000;	// PWM频率=10KHZ
    device.para.usart_baud = 115200;			// 串口波特率
    device_init(&device, LED | OLED | KEY | BUZZER | TIMER | MOTOR | MPU6050 | USART); // 初始化设备
	
	// 算法库初始化
	imu_init(&algo_imu, 0.005f, &mpu6050_data.acc.x, &mpu6050_data.acc.y, &mpu6050_data.acc.z, &mpu6050_data.gyro.x, &mpu6050_data.gyro.y, &mpu6050_data.gyro.z);
	algo_pid_init(&algo_pid_pitch_angle, 2.5f, 0.01f, 10.0f, 1000);
	algo_pid_init(&algo_pid_roll_angle, 2.5f, 0.01f, 10.0f, 1000);
	algo_pid_init(&algo_pid_yaw_angle, 2.5f, 0.01f, 10.0f, 1000);
	algo_pid_init(&algo_pid_pitch_gyro, 6.5f, 0.01f, 20.0f, 1000);
	algo_pid_init(&algo_pid_roll_gyro, 6.5f, 0.01f, 20.0f, 1000);
	algo_pid_init(&algo_pid_yaw_gyro, 6.5f, 0.01f, 20.0f, 1000);
}

int main()
{
	// 系统初始化
    system_init();
	
	// 主循环
    while(1) 
	{
		// 主循环状态机
		switch (g_main_state) 
		{
			// 获取信息
			case GET_INFO:
				g_main_state = SHOW_INFO;
				break;
			
			// 显示信息
			case SHOW_INFO:	
				user_interface();
				g_main_state = SEND_INFO;
				break;
			
			// 发送信息
			case SEND_INFO:
				g_main_state = GET_INFO;
				break;
		}
	}	
}
